#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTServo,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     rightFront,    tmotorTetrix, PIDControl, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     rightBack,     tmotorTetrix, PIDControl, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     leftBack,      tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     leftFront,     tmotorTetrix, PIDControl)
#pragma config(Motor,  mtr_S2_C2_1,     beaterBar,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     lift,          tmotorTetrix, openLoop, encoder)
#pragma config(Servo,  srvo_S2_C1_1,    backRightServo,       tServoStandard)
#pragma config(Servo,  srvo_S2_C1_2,    backLeftServo,        tServoStandard)
#pragma config(Servo,  srvo_S2_C1_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

void initializeRobot(){
	nMotorEncoder[lift] = 0;
  return;
}
bool liftCheck(float x){
	if(nMotorEncoder[lift] >= 0 + x && nMotorEncoder[lift] <= -(11.25*360) + x){
		return false;
	}
	return true;
}
task main()
{
	short zero = 6;
	float liftmod = 75;
	initializeRobot();
	waitForStart();
	while(true){
		getJoystickSettings(joystick);

		if(joystick.joy1_y1 > zero){ //up
			if(liftCheck(liftmod)){
				motor[lift] = liftmod;
			}
		}else if(joystick.joy1_y1 < -zero){ //down
			if(liftCheck(liftmod)){
				motor[lift] = -liftmod;
			}
		}else{
			motor[lift] = 0;
		}
	}


}
